package com.grt192.benchtest.mechanism;

import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Mechanism;
import com.grt192.sensor.GRTSwitch;

/**
 *
 * @author anand
 */
public class BenchKicker extends Mechanism {

    private GRTJaguar motor;
    private GRTSwitch limit;

    private boolean debug;
    
    public BenchKicker(int port, int limiter, boolean debug){
        motor = new GRTJaguar(port);
        motor.start();
        addActuator("Motor", motor);
        limit = new GRTSwitch(limiter, 25, "kickLimit");
        limit.start();
        addSensor("switch", limit);
        this.debug = debug;
    }

    public void spinForward(){
        if(debug)
            System.out.println("K FWD");
        motor.enqueueCommand(1.0);
    }

    public void spinReverse(){
        if(debug)
            System.out.println("K REV");
        motor.enqueueCommand(-1.0);
    }

    public void stop(){
        if(debug)
            System.out.println("K HALT");
        motor.halt();
    }
    
}
